#include "app.h"

void app_Init(void) {
	control.ticks = 0;
	control.key = 0;

	led1_Init();
	led2_Init();
	led3_Init();
	led4_Init();
	led5_Init();
	led6_Init();

	relay_Init();

	flash_Load();

 	// se os valores lidos, seja da flash ou padrões, não estão corretos trava o sistema e sinaliza o erro
 	int check = check_Values();
 	if (check!=pdOK) showVicError(check);

 	servo_Init();

	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		control.joy[x].updated = pdFALSE;
		control.joy[x].pos = control.servo[x].posNew;
	}

	lcd_SetCursor(0, 0);
	lcd_printf("rexARM");
}

// Aqui descre, ou chame, códigos que controlam tempos ou que sejam executados em tempos reguladores
void app_Tick(void) {
	// ATENÇÃO:  tempo de execução de TODOS os TICKS dentro dessa função NÃO pode ser superior a 1ms
	ledError_Tick();
	beep_Tick();
	gKey_Tick();
	relay_Tick();
	//ssp1_Init(SSP0_MASTER, SSP0_CPOL_H|SSP0_CPHA_H|SSP0_DSS_8); // inicar o barramento SPI a 400Khz
	joy_Tick();
	//ssp1_Init(SSP0_MASTER, SSP0_STANDARD); // inicar o barramento SPI a 400Khz

	// LEITURA DO TECLADO a cada 3ms
	static volatile uint timeReadKey = 0;
	static int bufKey[3];
	static uint idx = 0;
	if (timeReadKey) timeReadKey--;
	if (timeReadKey == 0) {
		timeReadKey = 3;

		// fazer debounce no teclado
		bufKey[idx%3] = (sw0_PIN&sw0_NUMPIN)?SW0:0;
		bufKey[idx%3] |= (sw1_PIN&sw1_NUMPIN)?SW1:0;
		bufKey[idx%3] |= (sw2_PIN&sw2_NUMPIN)?SW2:0;

		if ( (bufKey[0] == bufKey[1]) && (bufKey[1] == bufKey[2]) ) {
			control.key = bufKey[2]&0x7; // para que não pegar o botão de reconfigure
		}
		if (idx == 2) idx = 0; else idx++;
	}

	control.ticks++; // Contador de ticks do sistema
}


// Aqui descre, ou chame, códigos que não controlam tempos ou que não sejam executados em tempo reguladores, pelo menos diretamente
void app_Process(void) {
//	static uint ticks_d = 0;
//	if (!( (control.ticks%100==0) && (control.ticks != ticks_d) )) return;
//	ticks_d = control.ticks;

	int buttons = joy_Read();
	if (buttons == 0) return;

	lcd_SetCursor(0,1);

	#define STEP 10 // DEVE SER MULTIPLO DE 10

	tParServo* parServo;
	switch (buttons) {
	case joypsKEY_UP:
		parServo = (tParServo*)control.servo[1].par;
		if (control.joy[1].pos+STEP <= (uint)parServo->max) {
			control.joy[1].updated = pdTRUE;
			control.joy[1].pos+=STEP;
			lcd_printf("FRONT: %d", control.joy[1].pos);
		} else lcd_printf("FRONT: %d END", control.joy[1].pos);
		break;
	case joypsKEY_DOWN:
		parServo = (tParServo*)control.servo[1].par;
		if (control.joy[1].pos-STEP >= (uint)parServo->min) {
			control.joy[1].updated = pdTRUE;
			control.joy[1].pos-=STEP;
			lcd_printf("BACK: %d", control.joy[1].pos);
		} else lcd_printf("BACK: %d END", control.joy[1].pos);
		break;

	case joypsKEY_LEFT:
		parServo = (tParServo*)control.servo[0].par;
		if (control.joy[0].pos+STEP <= (uint)parServo->max) {
			control.joy[0].updated = pdTRUE;
			control.joy[0].pos+=STEP;
			lcd_printf("RIGHT: %d", control.joy[0].pos);
		} else lcd_printf("RIGHT: %d END", control.joy[0].pos);
		break;
	case joypsKEY_RIGHT:
		parServo = (tParServo*)control.servo[0].par;
		if (control.joy[0].pos-STEP >= (uint)parServo->min) {
			control.joy[0].updated = pdTRUE;
			control.joy[0].pos-=STEP;
			lcd_printf("LEFT: %d", control.joy[0].pos);
		} else lcd_printf("LEFT: %d END", control.joy[0].pos);
		break;

	case joypsKEY_TRIANGLE:
		parServo = (tParServo*)control.servo[3].par;
		if (control.joy[3].pos+STEP <= (uint)parServo->max) {
			control.joy[3].updated = pdTRUE;
			control.joy[3].pos+=STEP;
			lcd_printf("UP: %d", control.joy[3].pos);
		} else lcd_printf("UP: %d END", control.joy[3].pos);
		break;

	case joypsKEY_X:
		parServo = (tParServo*)control.servo[3].par;
		if (control.joy[3].pos-STEP >= (uint)parServo->min) {
			control.joy[3].updated = pdTRUE;
			control.joy[3].pos-=STEP;
			lcd_printf("DOWN: %d", control.joy[3].pos);
		} else lcd_printf("DOWN: %d END", control.joy[3].pos);
		break;

	case joypsKEY_CIRCLE:
		parServo = (tParServo*)control.servo[4].par;
		if (control.joy[4].pos+STEP <= (uint)parServo->max) {
			control.joy[4].updated = pdTRUE;
			control.joy[4].pos+=STEP;
			lcd_printf("UP2: %d", control.joy[4].pos);
		} else lcd_printf("UP2: %d END", control.joy[4].pos);
		break;
	case joypsKEY_SQUARE:
		parServo = (tParServo*)control.servo[4].par;
		if (control.joy[4].pos-STEP >= (uint)parServo->min) {
			control.joy[4].updated = pdTRUE;
			control.joy[4].pos-=STEP;
			lcd_printf("DOWN2: %d", control.joy[4].pos);
		} else lcd_printf("DOWN2: %d END", control.joy[4].pos);
		break;

	case joypsKEY_L1:
		parServo = (tParServo*)control.servo[5].par;
		if (control.joy[5].pos-STEP >= (uint)parServo->min) {
			control.joy[5].updated = pdTRUE;
			control.joy[5].pos-=STEP;
			lcd_printf("WRIST R: %d", control.joy[5].pos);
		} else lcd_printf("WRIST R: %d END", control.joy[5].pos);
		break;

	case joypsKEY_R1:
		parServo = (tParServo*)control.servo[5].par;
		if (control.joy[5].pos+STEP <= (uint)parServo->max) {
			control.joy[5].updated = pdTRUE;
			control.joy[5].pos+=STEP;
			lcd_printf("WRIST L: %d", control.joy[5].pos);
		} else lcd_printf("WRIST L: %d END", control.joy[5].pos);
		break;

	case joypsKEY_L2:
		servo_Hand(handOPEN);
		lcd_printf("HAND CLOSE");
		break;

	case joypsKEY_R2:
		servo_Hand(handCLOSE);
		lcd_printf("HAND OPEN");
		break;

	case joypsKEY_SELECT:
		lcd_printf("Pressed SELECT");
		break;

	case joypsKEY_START:
		servo_Parking();
		break;
	}

	uint x; for(x=0;x<SERVO_N_MODULE;x++) {
		// Como servo 3 espelho do servo 2, não vamos atualizar o servo 3, e o servo 7 é o controle da mão
		if ( (x==2) || (x==6) ) continue;
		if (control.joy[x].updated) {
			control.joy[x].updated = pdFALSE;
			servo_SetPos(x+1, control.joy[x].pos);
			//lprintf("%d:%d"CMD_TERMINATOR, x+1, control.joy[x].pos);
		}
	}
}
